import cv2
import numpy as np
import os

def detect_and_annotate_chessboard(image_path, output_dir, pattern_size=(7, 7)):
    # 读取图像
    image = cv2.imread(image_path)
    if image is None:
        raise FileNotFoundError(f"Image not found at {image_path}")

    # 转换为灰度图像
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

    # 检测棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)

    # 如果检测到角点
    if ret:
        # 细化角点
        corners_subpix = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

        # 在原图上标注角点
        image_with_corners = cv2.drawChessboardCorners(image, pattern_size, corners_subpix, ret)

        # 指定保存路径
        if not os.path.exists(output_dir):
            os.makedirs(output_dir)
        output_path = os.path.join(output_dir, "detect_corner_img.jpg")

        # 保存标注后的图像
        cv2.imwrite(output_path, image_with_corners)

        # 输出角点坐标
        print(f"Chessboard corners coordinates (in pixels):")
        for corner in corners_subpix.astype(int):  # 转换为整数坐标以便打印
            print(corner.ravel())  # 将二维坐标转换为一维数组并打印
    else:
        print("Chessboard corners not found.")

# 设置角点检测的迭代终止条件
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# 调用函数，传入图像路径和输出目录
image_path = 'images_processing/img_out/undistorted_img.jpg'  # 替换为你的棋盘格图像路径
output_dir = 'images_processing/img_out/'  # 替换为你希望保存输出图像的目录
detect_and_annotate_chessboard(image_path, output_dir)